import rospy
import pyquaternion
from gazebo_msgs.srv import SetModelState
from gazebo_msgs.msg import ModelState
from math import pi,cos,sin
from geometry_msgs.msg import *
model_state_srv = rospy.ServiceProxy("gazebo/set_model_state",SetModelState)
rospy.init_node('set_pose')
i = 0
while i<10000:
    i = i+1
    state_msg = ModelState()
    state_msg.model_name = "FordPickUp"
    state_msg.pose = Pose(Point(x=0+20*cos(2*pi/2000*i), y=0+20*sin(2*pi/2000*i), z=-1),   pyquaternion.Quaternion(axis=[0, 0, 1], angle=2*pi/2000*i + pi))
    state_msg.reference_frame = "solo"
    # model_state_srv(state_msg)

    state_msg.model_name = "FordPickUp_clone_0"
    state_msg.pose = Pose(Point(x=0+20*cos(2*pi/2000*i+pi/4), y=0+20*sin(2*pi/2000*i+pi/4), z=-1),   pyquaternion.Quaternion(axis=[0, 0, 1], angle=2*pi/2000*i + pi+pi/4))
    model_state_srv(state_msg)

    # state_msg.model_name = "FordPickUp_clone"
    # state_msg.pose = Pose(Point(x=0+20*cos(2*pi/2000*i+2*pi/4), y=0+20*sin(2*pi/2000*i+2*pi/4), z=-1),   pyquaternion.Quaternion(axis=[0, 0, 1], angle=2*pi/2000*i + pi + 2*pi/4))
    # model_state_srv(state_msg)

    # state_msg.model_name = "FordPickUp_clone_0_clone"
    # state_msg.pose = Pose(Point(x=0+20*cos(2*pi/2000*i+3*pi/4), y=0+20*sin(2*pi/2000*i+3*pi/4), z=-1),   pyquaternion.Quaternion(axis=[0, 0, 1], angle=2*pi/2000*i + pi + 3*pi/4))
    # model_state_srv(state_msg)

    state_msg = ModelState()
    state_msg.model_name = "solo"
    state_msg.pose = Pose(Point(x=i/100, y=0, z=1),   pyquaternion.Quaternion(axis=[0, 0, 1], angle=0))

    model_state_srv(state_msg)